/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Robot;


import lejos.nxt.*;
import lejos.robotics.navigation.Pilot;

/**
 *
 * @author zach
 */
public class Configureation {
    /**
     * constains don't mess wtih these
     */
    public static final int touch_sensor = 0;
    public static final int compass_sensor = 1;
    public static final int sound_sensor = 2;
    public static final int ultra_sound_sensor = 3;
    public static final int light_sensor = 4;
    public static final int null_sensor = -1;
    public static final short null_motor = -1;
    public static final short leftDrive_motor = 0;
    public static final short rightDrive_motor = 1;
    public static final short centerDrive_motor = 2;
    public static final short turning_motor = 3;
    public static final short sensor_motor = 4;
    /**
     * robot configration set the sensor you are using.
     * if you are getting null pointers later on you most likely forgot
     * this step
     */
    public static final int sensor_one = light_sensor;
    public static final int sensor_two = ultra_sound_sensor;
    public static final int sensor_three = compass_sensor;
    public static final int sensor_four = null_sensor;

    public static final short motor_one = leftDrive_motor;
    public static final short motor_two = sensor_motor;
    public static final short motor_three = rightDrive_motor;

    public static final float leftWheelDiameter=58f;
    public static final float rightWheelDiameter=58f;
    public static final float trackWidth=160f;
    public static final boolean reverse=true;

    /**
     * end of robot configuration
     */

    /**
     * @return the center motor, null if it hasn't been set
     */

    public static Motor getDriveMotor(){
        if(motor_one==centerDrive_motor)
            return Motor.A;
        else if(motor_two==centerDrive_motor)
            return Motor.B;
        else if(motor_three==centerDrive_motor)
            return Motor.C;
        else return null;
    }

    public static Motor getLeftDriveMotor(){
        if(motor_one==leftDrive_motor)
            return Motor.A;
        else if(motor_two==leftDrive_motor)
            return Motor.B;
        else if(motor_three==leftDrive_motor)
            return Motor.C;
        else return null;
    }
    public static Motor getRightDriveMotor(){
        if(motor_one==rightDrive_motor)
            return Motor.A;
        else if(motor_two==rightDrive_motor)
            return Motor.B;
        else if(motor_three==rightDrive_motor)
            return Motor.C;
        else return null;
    }
    public static Motor[] getDriveMotors(){
        int size=0;
        Motor[] out;
        if(motor_one!=null_motor||motor_one!=sensor_motor||motor_one!=turning_motor)
        size++;
        if(motor_two!=null_motor||motor_two!=sensor_motor||motor_two!=turning_motor)
        size++;
        if(motor_three!=null_motor||motor_three!=sensor_motor||motor_three!=turning_motor)
        size++;

        out=new Motor[size];
        size=0;
        if(motor_one==leftDrive_motor){
            out[size]=Motor.A;
            size++;
        } else if(motor_two==leftDrive_motor){
            out[size]=Motor.B;
            size++;
        }else if(motor_three==leftDrive_motor){
            out[size]=Motor.C;
            size++;
        }
        if(motor_one==centerDrive_motor){
            out[size]=Motor.A;
            size++;
        } else if(motor_two==centerDrive_motor){
            out[size]=Motor.B;
            size++;
        }else if(motor_three==centerDrive_motor){
            out[size]=Motor.C;
            size++;
        }
        if(motor_one==rightDrive_motor){
            out[size]=Motor.A;
            size++;
        } else if(motor_two==rightDrive_motor){
            out[size]=Motor.B;
            size++;
        }else if(motor_three==rightDrive_motor){
            out[size]=Motor.C;
            size++;
        }
        return out;
    }

    public static Motor getTurningMotor(){
        if(motor_one==rightDrive_motor){
            return Motor.A;
        } else if(motor_two==rightDrive_motor){
            return Motor.B;
        }else if(motor_three==rightDrive_motor){
            return Motor.C;
        }
        return null;
    }

    public static Motor[] getSensorMotors(){
        int size=0;
        Motor[] out;
        if(motor_one==sensor_motor)
        size++;
        if(motor_two==sensor_motor)
        size++;
        if(motor_three==sensor_motor)
        size++;

        out=new Motor[size];
        size=0;
        if(motor_one==sensor_motor){
            out[size]=Motor.A;
            size++;
        }
        if(motor_two==sensor_motor){
            out[size]=Motor.B;
            size++;
        }
        
        return out;
    }

    private static Pilot pilot=null;

    public static Pilot getPilot(){
        if(pilot==null){
            pilot=new CompassPilot();
        }
        return pilot;
    }

    private static TouchSensor t1 = null;
    private static TouchSensor t2 = null;
    private static TouchSensor t3 = null;
    private static TouchSensor t4 = null;

    public static TouchSensor touchSensor() {
        return touchSensors()[0];
    }

    public static TouchSensor[] touchSensors() {
        TouchSensor[] out;
        int size = 0;
        if (sensor_one == touch_sensor) {
            if (t1 == null) {
                t1 = new TouchSensor(SensorPort.S1);
            }
            size++;
        } else if (sensor_two == touch_sensor) {
            if (t2 == null) {
                t2 = new TouchSensor(SensorPort.S2);
            }
            size++;
        } else if (sensor_three == touch_sensor) {
            if (t3 == null) {
                t3 = new TouchSensor(SensorPort.S3);
            }
            size++;
        } else if (sensor_four == touch_sensor) {
            if (t4 == null) {
                t4 = new TouchSensor(SensorPort.S4);
            }
            size++;
        }
        out = new TouchSensor[size];
        if (size == 0) {
            return null;
        } else {
            size = 0;
            if (sensor_one == touch_sensor) {
                out[size] = t1;
                size++;
            }
            if (sensor_two == touch_sensor) {
                out[size] = t2;
                size++;
            }
            if (sensor_three == touch_sensor) {
                out[size] = t3;
                size++;
            }
            if (sensor_four == touch_sensor) {
                out[size] = t4;
                size++;
            }

        }
        return out;
    }
    private static SDCSoundSensor s1 = null;
    private static SDCSoundSensor s2 = null;
    private static SDCSoundSensor s3 = null;
    private static SDCSoundSensor s4 = null;

    public static SDCSoundSensor soundSensor() {
        return soundSensors()[0];
    }

    public static SDCSoundSensor[] soundSensors() {
        SDCSoundSensor[] out;
        int size = 0;
        if (sensor_one == sound_sensor) {
            if (s1 == null) {
                s1 = new SDCSoundSensor(SensorPort.S1);
            }
            size++;
        } else if (sensor_two == sound_sensor) {
            if (s2 == null) {
                s2 = new SDCSoundSensor(SensorPort.S2);
            }
            size++;
        } else if (sensor_three == sound_sensor) {
            if (s3 == null) {
                s3 = new SDCSoundSensor(SensorPort.S3);
            }
            size++;
        } else if (sensor_four == sound_sensor) {
            if (s4 == null) {
                s4 = new SDCSoundSensor(SensorPort.S4);
            }
            size++;
        }
        out = new SDCSoundSensor[size];
        if (size == 0) {
            return null;
        } else {
            size = 0;
            if (sensor_one == sound_sensor) {
                out[size] = s1;
                size++;
            }
            if (sensor_two == sound_sensor) {
                out[size] = s2;
                size++;
            }
            if (sensor_three == sound_sensor) {
                out[size] = s3;
                size++;
            }
            if (sensor_four == sound_sensor) {
                out[size] = s4;
                size++;
            }
        }
        return out;
    }
    private static SDCLightSensor l1 = null;
    private static SDCLightSensor l2 = null;
    private static SDCLightSensor l3 = null;
    private static SDCLightSensor l4 = null;

    public static SDCLightSensor lightSensor() {
        return lightSensors()[0];
    }

    public static SDCLightSensor[] lightSensors() {
        SDCLightSensor[] out;
        int size = 0;
        if (sensor_one == light_sensor) {
            if (l1 == null) {
                l1 = new SDCLightSensor(SensorPort.S1);
            }
            size++;
        } else if (sensor_two == light_sensor) {
            if (l2 == null) {
                l2 = new SDCLightSensor(SensorPort.S2);
            }
            size++;
        } else if (sensor_three == light_sensor) {
            if (l3 == null) {
                l3 = new SDCLightSensor(SensorPort.S3);
            }
            size++;
        } else if (sensor_four == light_sensor) {
            if (l4 == null) {
                l4 = new SDCLightSensor(SensorPort.S4);
            }
            size++;
        }
        out = new SDCLightSensor[size];
        if (size == 0) {
            return null;
        } else {
            size = 0;
            if (sensor_one == light_sensor) {
                out[size] = l1;
                size++;
            }
            if (sensor_two == light_sensor) {
                out[size] = l2;
                size++;
            }
            if (sensor_three == light_sensor) {
                out[size] = l3;
                size++;
            }
            if (sensor_four == light_sensor) {
                out[size] = l4;
                size++;
            }
        }
        return out;
    }
    private static SDCUltraSoundSensor u1 = null;
    private static SDCUltraSoundSensor u2 = null;
    private static SDCUltraSoundSensor u3 = null;
    private static SDCUltraSoundSensor u4 = null;

    public static SDCUltraSoundSensor ultraSoundSensor() {
        return ultraSoundSensors()[0];
    }

    public static SDCUltraSoundSensor[] ultraSoundSensors() {
        SDCUltraSoundSensor[] out;
        int size = 0;
        if (sensor_one == ultra_sound_sensor) {
            if (u1 == null) {
                u1 = new SDCUltraSoundSensor(SensorPort.S1);
            }
            size++;
        } else if (sensor_two == ultra_sound_sensor) {
            if (u2 == null) {
                u2 = new SDCUltraSoundSensor(SensorPort.S2);
            }
            size++;
        } else if (sensor_three == ultra_sound_sensor) {
            if (u3 == null) {
                u3 = new SDCUltraSoundSensor(SensorPort.S3);
            }
            size++;
        } else if (sensor_four == ultra_sound_sensor) {
            if (u4 == null) {
                u4 = new SDCUltraSoundSensor(SensorPort.S4);
            }
            size++;
        }
        out = new SDCUltraSoundSensor[size];
        if (size == 0) {
            return null;
        } else {
            size = 0;
            if (sensor_one == ultra_sound_sensor) {
                out[size] = u1;
                size++;
            }
            if (sensor_two == ultra_sound_sensor) {
                out[size] = u2;
                size++;
            }
            if (sensor_three == ultra_sound_sensor) {
                out[size] = u3;
                size++;
            }
            if (sensor_four == ultra_sound_sensor) {
                out[size] = u4;
                size++;
            }
        }
        return out;
    }
    private static CompassSensor c1 = null;
    private static CompassSensor c2 = null;
    private static CompassSensor c3 = null;
    private static CompassSensor c4 = null;

    public static CompassSensor compassSensor() {
        return compassSensors()[0];
    }

    public static CompassSensor[] compassSensors() {
        CompassSensor[] out;
        int size = 0;
        if (sensor_one == compass_sensor) {
            if (c1 == null) {
                c1 = new CompassSensor(SensorPort.S1);
            }
            size++;
        } else if (sensor_two == compass_sensor) {
            if (c2 == null) {
                c2 = new CompassSensor(SensorPort.S2);
            }
            size++;
        } else if (sensor_three == compass_sensor) {
            if (c3 == null) {
                c3 = new CompassSensor(SensorPort.S3);
            }
            size++;
        } else if (sensor_four == compass_sensor) {
            if (c4 == null) {
                c4 = new CompassSensor(SensorPort.S4);
            }
            size++;
        }
        out = new CompassSensor[size];
        if (size == 0) {
            return null;
        } else {
            size = 0;
            if (sensor_one == compass_sensor) {
                out[size] = c1;
                size++;
            }
            if (sensor_two == compass_sensor) {
                out[size] = c2;
                size++;
            }
            if (sensor_three == compass_sensor) {
                out[size] = c3;
                size++;
            }
            if (sensor_four == compass_sensor) {
                out[size] = c4;
                size++;
            }
        }
        return out;
    }
    public static void wait(int time){
        try {
            Thread.sleep(time);
        } catch (InterruptedException ex) {

    }

    }
}
